/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: Position_Dummy.cpp,v 1.8 2009-08-12 11:48:29 amaula Exp $
 *
 * \file
 * \brief Dummy implementation for Position Server interface.
 * \author Antti Maula <antti.maula@tkk.fi>
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 *
 * This implementation can act as an example / codebase for any
 * Position server based module implementation. This code follows a
 * pseudogood coding style, and also provides a basic parameter
 * parser that can be directly used with slight modifications.
 * 
 */
#include "owndebug.h"
#include "PositionServer.hpp"
#include "binbag.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
#define MODE_INCREMENTAL   1
#define MODE_CIRCLE        2
#define MODE_RANDOM        3
#define MODE_SET					 4
//*****************************************************************************
static bool run = true;

static int verbose = 1;
static int mode = MODE_INCREMENTAL;

static std::string gimnetAP = "asrobo.hut.fi";
static int gimnetAPPort = 50002;
static std::string gimnetName = "";
static std::string maciGroupName;
static std::string interfaceInstanceName = "Dummy";
//*****************************************************************************
static std::string modeParam;
//*****************************************************************************
using namespace MaCI;
//*****************************************************************************

void showUsage()
{
  fprintf(stderr,"\n");
  fprintf(stderr,"Usage:\tPosition_Dummy [OPTIONS]\n");
  fprintf(stderr,"  [-u addr]\t\t\tUse 'addr' as GIMnet AccessPoints address\n");
  fprintf(stderr,"  [-p port]\t\t\tUse 'port' as GIMnet AccessPoints port\n");
  fprintf(stderr,"  [-n name]\t\t\tUse 'name' as this nodes GIMnet name\n");
  fprintf(stderr,"  [-G name]\t\t\tUse 'name' as MaCIGroup for this service\n");
  fprintf(stderr,"  [-I name]\t\t\tUse 'name' as Instancename for this service\n");
  fprintf(stderr,"  [-M mode]\t\t\tSelect mode. (incremental, circle, random)\n");
  fprintf(stderr,"  [-P prm]\t\t\tMode parameter. (Mode dependent)\n");
  fprintf(stderr,"  \t\t\t\t- For 'incremental', give vector in format: '<x>,<y>' (m/s, m/s)\n");
  fprintf(stderr,"  \t\t\t\t- For 'circle', give radius and trajectory speed in format: '<radius>,<s>' (m, m/s)\n");
  fprintf(stderr,"  \t\t\t\t- For 'random', give start heading and trajectory speed: '<heading>,<speed>' (deg, m/s)\n");
  fprintf(stderr,"  \t\t\t\t- For 'set', gives the static pose and sets to setted\n");

  fprintf(stderr,"\n");
  
  fprintf(stderr,"  [-v]\t\t\t\tIncrease verbosity by 1\n");
  fprintf(stderr,"  [-q]\t\t\t\tQuiet mode\n");
  fprintf(stderr, "\n\n");
}
//*****************************************************************************

static void ExecuteModeIncremental(Position::CPositionServer &ps)
{
  // namespace to use
  using namespace MaCI::Position;
    
  // Flo
  MaCI::EMaCIError e;
 
  ////// Execute test section ///////
  int interval = 200;
  MaCI::Common::TTimestamp stamp;
  float x=0,y=0,a=0;
  float vx=0,vy=0,va=0;
  float p = 0;
  
  // Default delta values.
  float dx = 0.5, dy = 0.5;
  
  // Do calc
  if (modeParam.size()) {
    if (sscanf(modeParam.c_str(),"%f,%f",&dx, &dy) != 2) {
      dPrint(ODERROR,"Failed to parse modeParam '%s'. Fatal error, exit!", modeParam.c_str());
      exit(1);
    }
  }

  // Data elements.
  CPositionData pos;
  pos.CreateInternalBinBag();
      
  double loop_begin = ownTime_get_s();
    x = 0;y=0; a=0;
  while(run) {
    const double time_used = ownTime_get_s_since(loop_begin);
    loop_begin = ownTime_get_s(); // Immediately update the start for the next round.

    // Modify data
    //x += time_used * dx;
    //y += time_used * dy;

    //a = atan2(y, x);
    vx = 1 / (1 + (x * a));
    vy = 1 / (1 + (y * a));
    va = 0.00;
    p = 1.00;

    stamp.SetToCurrentTime();

    // Assign new data
    pos.Reset();
    pos.SetTimestamp(stamp);
    pos.SetPose2D(TPose2D(x,y,a));
    pos.SetVariance2D(TVariance2D(vx,vy,va));
    // pos.SetVariance2D(TPose2D(vx,vy,va)); // Also possible like this.
    pos.SetProbability(TProbability(p));

    // Send data
    ps.SendPositionEvent(pos);
    dPrint(1,"New position data sent!");
    pos.Print(1);

    // Do wait.
    CPositionData pd;
    e = ps.WaitCommand(pd, interval);
    if (e == MaCI::KMaCIOK) {
      dPrint(1,"Got Command!");
      const TCommand *cmd = pd.GetCommand();
      if (cmd) {
        switch(cmd->cmd) {
        case  KCommandInitialize:
          dPrint(1,"Got Command - 'Initialize'");
          break;
        case KCommandSetPose2D:
          const TPose2D *pose = pd.GetPose2D();
          dPrint(1,"Got Command - SetPose2D = (%.2f, %.2f, %.4f)",
                 pose->x, pose->y, pose->a);
	  x=pose->x;
	  y=pose->y;
	  a=pose->a;
          break;
        }
      } else {
        dPrint(1,"No Command in Command!?");
      }
          
      // If user really wants correct SLEEP time; should call Sleep()
      // with remainder of interval here. We are not interested
      // because this is an example program :)
    } 
  }
}
//*****************************************************************************

static void ExecuteModeCircle(Position::CPositionServer &ps)
{
  // namespace to use
  using namespace MaCI::Position;
    
  // Flo
  MaCI::EMaCIError e;
 
  ////// Execute test section ///////
  int interval = 200;
  MaCI::Common::TTimestamp stamp;
  double angle = 0;
  float x=0,y=0,a=0;
  float vx=0,vy=0,va=0;
  float p = 0;
  
  // Default delta values.
  float r = 3.0, rs = 0.5;
  
  // Do calc
  if (modeParam.size()) {
    if (sscanf(modeParam.c_str(),"%f,%f",&r, &rs) != 2) {
      dPrint(ODERROR,"Failed to parse modeParam '%s'. Fatal error, exit!", modeParam.c_str());
      exit(1);
    }
  }

  // Data elements.
  CPositionData pos;
  pos.CreateInternalBinBag();
      
  double loop_begin = ownTime_get_s();
  while(run) {
    const double time_used = ownTime_get_s_since(loop_begin);
    loop_begin = ownTime_get_s(); // Immediately update the start for the next round.

    // Modify data
    // Calculate difference in angle,
    const double delta_angle = (rs * time_used) / r;

    // Increment the current angle.
    angle += delta_angle;

    // Check overflow
    if (angle > 2*M_PI) angle -= 2*M_PI;
    else if (angle < -2*M_PI) angle += 2*M_PI;

    // Calculate X and Y based on angle.
    x = sin(angle) * r;
    y = cos(angle) * r;
    a = -angle;

    vx = 1 / (1 + (x * a));
    vy = 1 / (1 + (y * a));
    va = 0.00;
    p = 1.00;

    stamp.SetToCurrentTime();

    // Assign new data
    pos.Reset();
    pos.SetTimestamp(stamp);
    pos.SetPose2D(TPose2D(x,y,a));
    pos.SetVariance2D(TVariance2D(vx,vy,va));
    // pos.SetVariance2D(TPose2D(vx,vy,va)); // Also possible like this.
    pos.SetProbability(TProbability(p));

    // Send data
    ps.SendPositionEvent(pos);
    dPrint(1,"New position data sent!");
    pos.Print(1);

    // Do wait.
    CPositionData pd;
    e = ps.WaitCommand(pd, interval);
    if (e == MaCI::KMaCIOK) {
      dPrint(1,"Got Command!");
      const TCommand *cmd = pd.GetCommand();
      if (cmd) {
        switch(cmd->cmd) {
        case  KCommandInitialize:
          dPrint(1,"Got Command - 'Initialize'");
          break;
        case KCommandSetPose2D:
          const TPose2D *pose = pd.GetPose2D();
          dPrint(1,"Got Command - SetPose2D = (%.2f, %.2f, %.4f)",
                 pose->x, pose->y, pose->a);
          break;
        }
      } else {
        dPrint(1,"No Command in Command!?");
      }
          
      // If user really wants correct SLEEP time; should call Sleep()
      // with remainder of interval here. We are not interested
      // because this is an example program :)
    } 
  }
}
//*****************************************************************************

static void ExecuteModeRandom(Position::CPositionServer &ps)
{
  // namespace to use
  using namespace MaCI::Position;
  
  ownRandom_init_time();
  
  // Flo
  MaCI::EMaCIError e;
 
  ////// Execute test section ///////
  int interval = 200;
  MaCI::Common::TTimestamp stamp;
  float x=0,y=0,a=0;
  float vx=0,vy=0,va=0;
  float p = 0;
  
  // Control values.
  float heading = 1.5;
  float speed = 1;
  

  // Do calc
  if (modeParam.size()) {
    if (sscanf(modeParam.c_str(),"%f,%f",&heading, &speed) != 2) {
      dPrint(ODERROR,"Failed to parse modeParam '%s'. Fatal error, exit!", modeParam.c_str());
      exit(1);
    } else {
      // Convert degrees to radians
      heading = DEG2RAD(heading);
    }
  }

  // Data elements.
  CPositionData pos;
  pos.CreateInternalBinBag();
      
  double loop_begin = ownTime_get_s();
  while(run) {
    const double time_used = ownTime_get_s_since(loop_begin);
    loop_begin = ownTime_get_s(); // Immediately update the start for the next round.

    // Update angle.
    heading += ownRandom_get_range(-0.25, 0.5);
    // Check overflow
    if (heading > 2*M_PI) heading -= 2*M_PI;
    else if (heading < -2*M_PI) heading += 2*M_PI;

    // Update change engine.
    x += time_used * speed * cos(heading);
    y += time_used * speed * sin(heading);
    a = heading;
    
    // Set some fillers.
    vx = 1 / (1 + (x * a));
    vy = 1 / (1 + (y * a));
    va = 0.00;
    p = 1.00;
    
    stamp.SetToCurrentTime();

    // Assign new data
    pos.Reset();
    pos.SetTimestamp(stamp);
    pos.SetPose2D(TPose2D(x,y,a));
    pos.SetVariance2D(TVariance2D(vx,vy,va));
    // pos.SetVariance2D(TPose2D(vx,vy,va)); // Also possible like this.
    pos.SetProbability(TProbability(p));

    // Send data
    ps.SendPositionEvent(pos);
    dPrint(1,"New position data sent!");
    pos.Print(1);

    // Do wait.
    CPositionData pd;
    e = ps.WaitCommand(pd, interval);
    if (e == MaCI::KMaCIOK) {
      dPrint(1,"Got Command!");
      const TCommand *cmd = pd.GetCommand();
      if (cmd) {
        switch(cmd->cmd) {
        case  KCommandInitialize:
          dPrint(1,"Got Command - 'Initialize'");
          break;
        case KCommandSetPose2D:
          const TPose2D *pose = pd.GetPose2D();
          dPrint(1,"Got Command - SetPose2D = (%.2f, %.2f, %.4f)",
                 pose->x, pose->y, pose->a);
          break;
        }
      } else {
        dPrint(1,"No Command in Command!?");
      }
          
      // If user really wants correct SLEEP time; should call Sleep()
      // with remainder of interval here. We are not interested
      // because this is an example program :)
    } 
  }
}
//*****************************************************************************
//*****************************************************************************

static void ExecuteModeSetPose(Position::CPositionServer &ps)
{
  // namespace to use
  using namespace MaCI::Position;
  
  ownRandom_init_time();
  
  // Flo
  MaCI::EMaCIError e;
 
  ////// Execute test section ///////
  int interval = 200;
  MaCI::Common::TTimestamp stamp;
  float x=30,y=20,a=0;
  float vx=0,vy=0,va=0;
  float p = 0;
  
  // Control values.
  float heading = 1.5;
  float speed = 1;
  // Data elements.
  CPositionData pos;
  pos.CreateInternalBinBag();
      

  
  double loop_begin = ownTime_get_s();
  while(run) {
    const double time_used = ownTime_get_s_since(loop_begin);
    loop_begin = ownTime_get_s(); // Immediately update the start for the next round.

    
    stamp.SetToCurrentTime();

    // Assign new data
    pos.Reset();
    pos.SetTimestamp(stamp);
    pos.SetPose2D(TPose2D(x,y,a));
    pos.SetVariance2D(TVariance2D(vx,vy,va));
    // pos.SetVariance2D(TPose2D(vx,vy,va)); // Also possible like this.
    pos.SetProbability(TProbability(p));

    // Send data
    ps.SendPositionEvent(pos);
    dPrint(1,"New position data sent!");
    pos.Print(1);

    // Do wait.
    CPositionData pd;
    e = ps.WaitCommand(pd, interval);
    if (e == MaCI::KMaCIOK) {
      dPrint(1,"Got Command!");
      const TCommand *cmd = pd.GetCommand();
      if (cmd) {
        switch(cmd->cmd) {
        case  KCommandInitialize:
          dPrint(1,"Got Command - 'Initialize'");
          break;
        case KCommandSetPose2D:
          const TPose2D *pose = pd.GetPose2D();
          dPrint(1,"Got Command - SetPose2D = (%.2f, %.2f, %.4f)",
                 pose->x, pose->y, pose->a);
								 x = pose->x; y=pose->y;a = pose->a;
			
          break;
        }
      } else {
        dPrint(1,"No Command in Command!?");
      }
          
      // If user really wants correct SLEEP time; should call Sleep()
      // with remainder of interval here. We are not interested
      // because this is an example program :)
    } 
  }
}
//*****************************************************************************




static int ExecuteModule(gimi::GIMI &g)
{
  // namespace to use
  using namespace MaCI::Position;
  
  MaCICtrl::CMaCICtrlServer mcs(&g);
  Position::CPositionServer ps(&mcs, 0, "Very Very Dummy"); 
  
    // Set MaCIGroup.
  mcs.SetGroupName(maciGroupName);

  // Set Identification parameters (MaCISL identification)
  ps.SetInterfaceInstanceName(interfaceInstanceName);
    

  MaCI::EMaCIError e;
  if ( (e = ps.Open()) != MaCI::KMaCIOK) {
    dPrint(1,"Failed to Open PositionServer instance! (%s)",
           GetErrorStr(e).c_str());
      
  } else {
    
    // Do fast test for timeoutless Wait's. This is just for internal
    // testing, so don't include in your application :)
    for (unsigned int i=0; i < 100; ++i) {
      CPositionData pd;
      
      // Test zero timeout
      e = ps.WaitCommand(pd, 0);
      if (e == MaCI::KMaCIOK) {
        dPrint(1,"Got Command with Zero timeout!");
        
      } else if (e == MaCI::KMaCITimeout) {
        dPrint(1,"No commands available.");
        
      } 
    }
    
    switch(mode) {
    case MODE_INCREMENTAL: 
      ExecuteModeIncremental(ps);
      break;
    case MODE_CIRCLE:
      ExecuteModeCircle(ps);
      break;
    case MODE_RANDOM:
      ExecuteModeRandom(ps);
      break;
		case MODE_SET:
			ExecuteModeSetPose(ps);
		break;
    }

  }      
  return 0;
}
//*****************************************************************************

int main(int argc, char *argv[]) 
{
  // Objects
  gimi::GIMI g;

  // Init Debug lib
  debugInit();
  debugSetGlobalDebugLvl(4);
  debugSetLogFilename("Position_Dummy.log");

  // Greet
  fprintf(stderr,
	  "\nPosition_Dummy - MaCI Position Interface Dummy Implementation - $Revision: 1.8 $\n");

  // Parse parameters
  bool parse = true;
  while(parse) {
    int c;
    c = getopt(argc, argv, "P:M:I:G:n:u:p:hqv");
    switch(c) {

      /////////////////////////////////
      ////// Application specific /////
      /////////////////////////////////
    case 'P': {
      modeParam = optarg;
      break;
    }

    case 'M': {
      const std::string argstr(optarg);
      if ( argstr == "circle" ) {
        mode = MODE_CIRCLE;
      } else if ( argstr == "incremenatal" ) {
        mode = MODE_INCREMENTAL;
      } else if ( argstr == "random" ) {
        mode = MODE_RANDOM;
      }	
			 else if ( argstr == "set" ) {
        mode = MODE_SET;
      }
        break;
    }
      /////////////////////////////////
      ///// GIMnet parameters /////////
      /////////////////////////////////
    case 'I':
      interfaceInstanceName = optarg;
      dPrint(3,"Using InterfaceInstance Name '%s'", optarg);
      break;

    case 'G':
      maciGroupName = optarg;
      dPrint(3,"Using MaCIGroupName '%s'", optarg);
      break;

    case 'n':
      gimnetName = optarg;
      dPrint(3,"Using GIMnet Name '%s'", optarg);
      break;

    case 'u':
      gimnetAP = optarg;
      dPrint(3,"Using GIMnetAP address '%s'", optarg);
      break;

    case 'p':
      gimnetAPPort = atoi(optarg);
      dPrint(3,"Using GIMnetAP port '%d'", gimnetAPPort);
      break;

      //////////////////////////////////
      //// GENERIC:                 ////
      //////////////////////////////////
    case 'v':
      // Increase only if not previously set to 0 (by 'quiet')
      if (verbose > 0)
	debugSetGlobalDebugLvl(++verbose);
      break;
      
    case 'q':
      verbose = 0;
      debugSetGlobalDebugLvl(0);
      break;
      
    case 'h':
    case '?':
      showUsage();
      exit(1);
      break;
      
    case -1:
      parse = false;
      break;
    }
  }
  
  /////////////////////////////////////////////////////////////////////////////
  // Modify gimetName if unset (length == zero)
  /////////////////////////////////////////////////////////////////////////////
  if (gimnetName.empty()) {
    gimnetName = MaCI::GetStaticClientName(argv[0]+maciGroupName+interfaceInstanceName);
  }
  
  int r;
  if ( (r = g.connectToHubEx(gimnetAP, gimnetAPPort, gimnetName) ) != GIMI_OK) {
    dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
    
  } else {
    dPrint(2,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
    
    ExecuteModule(g);
  }


  return 0;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************


